﻿using Entitas;
using UnityEngine;

public sealed class MoveSystem : IExecuteSystem
{
    public void Execute()
    {
        var entities = Contexts.sharedInstance.game
            .GetGroup(GameMatcher.AllOf(GameMatcher.Transform, GameMatcher.Velocity, GameMatcher.Accelerate))
            .GetEntities();

        foreach (var e in entities)
        {
            float tickTime = Contexts.sharedInstance.game.tick.tickTime;
            Vector3 accelerate = e.accelerate.value;
            e.velocity.value += accelerate * tickTime;
            e.velocity.Truncate();

            e.transform.position += e.velocity.value * tickTime;
            EnforceNonePenetrationConstraint(e, entities);
            if (e.velocity.value.sqrMagnitude > 0.01f)
            {
                e.transform.forward = e.velocity.value.normalized;
            }
            
            e.transform.smootherForward.Update(e.transform.forward);
        }
    }

    private void EnforceNonePenetrationConstraint(GameEntity e, GameEntity[] containerOfEntities)
    {
        if (!e.hasColliderRadius)
        {
            return;
        }
        
        foreach (var curEntity in containerOfEntities)
        {
            if (curEntity == e)
            {
                continue;;
            }

            if (!curEntity.hasColliderRadius)
            {
                continue;
            }

            Vector3 toEntity = e.transform.position - curEntity.transform.position;
            float dist = toEntity.magnitude;
            float amountOfOverLap = curEntity.colliderRadius.radius + e.colliderRadius.radius - dist;
            if (amountOfOverLap >= 0)
            {
                e.transform.position += toEntity / dist * amountOfOverLap;
            }
        }
    }
}